// stereo_calib_improved.cpp
// 改进版立体标定（含更鲁棒的棋盘格检测、配对与调试保存）
// 编译示例（Linux）:
// g++ stereo_calib_improved.cpp -o stereo_calib_improved `pkg-config --cflags --libs opencv4` -std=c++17

#include <opencv2/opencv.hpp>
#include <iostream>
#include <filesystem>
#include <iomanip>
#include <sstream>
#include <vector>
#include <algorithm>
#include <map>

namespace fs = std::filesystem;

// 从文件名中提取最后的数字 ID（如 img_left_001.jpg -> 1）
// 如果无法解析返回 -1
int extractNumberFromFilename(const std::string &filename)
{
    // 寻找最后一个下划线或非数字之前的部分
    size_t end = filename.find_last_of('.');
    if (end == std::string::npos) return -1;
    // 从文件名开始到扩展名前一部分
    std::string base = filename.substr(0, end);
    // 从末尾向前找到第一个非数字位置
    int i = (int)base.size() - 1;
    while (i >= 0 && isdigit((unsigned char)base[i])) --i;
    std::string num = base.substr(i + 1);
    if (num.empty()) return -1;
    try {
        return std::stoi(num);
    } catch (...) {
        return -1;
    }
}

// Gamma 校正（gamma > 1 变暗, gamma < 1 变亮）
// 这里用于提升暗图的对比表现
cv::Mat applyGamma(const cv::Mat &src, double gamma)
{
    CV_Assert(gamma > 0);
    cv::Mat lut(1, 256, CV_8UC1);
    for (int i = 0; i < 256; i++) {
        lut.at<uchar>(i) = cv::saturate_cast<uchar>(std::pow(i / 255.0, gamma) * 255.0);
    }
    cv::Mat dst;
    cv::LUT(src, lut, dst);
    return dst;
}

// 预处理管线：输入彩色原图（BGR），输出用于检测的灰度图（单通道）
// 同时可选择保存中间调试图像
cv::Mat preprocessForChessboard(const cv::Mat &imgBGR, bool saveDebug = false, const std::string &debugPrefix = "")
{
    cv::Mat gray;
    if (imgBGR.channels() == 3)
        cv::cvtColor(imgBGR, gray, cv::COLOR_BGR2GRAY);
    else
        gray = imgBGR.clone();

    // 1) gamma 校正（稍微变亮，针对暗图） - gamma 值可调
    cv::Mat gammaImg = applyGamma(gray, 0.8); // <1 更亮

    // 2) CLAHE（对比度受限自适应直方图均衡）
    cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
    cv::Mat claheImg;
    clahe->apply(gammaImg, claheImg);

    // 3) 双边滤波（保边缘的去噪）
    cv::Mat denoised;
    cv::bilateralFilter(claheImg, denoised, 9, 75, 75);

    // 4) 可选的形态学闭操作（填补小黑洞）
    cv::Mat morph;
    cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
    cv::morphologyEx(denoised, morph, cv::MORPH_CLOSE, kernel);

    if (saveDebug)
    {
        if (!debugPrefix.empty()) {
            cv::imwrite(debugPrefix + "_gamma.png", gammaImg);
            cv::imwrite(debugPrefix + "_clahe.png", claheImg);
            cv::imwrite(debugPrefix + "_denoised.png", denoised);
            cv::imwrite(debugPrefix + "_morph.png", morph);
        }
    }

    return morph;
}

// 更鲁棒的棋盘角点检测：优先尝试 findChessboardCornersSB（若可用），否则使用 findChessboardCorners 的多尺度尝试
// 返回值：是否找到角点；corners 输出为原图坐标系下的角点（与输入原始图像对应）
bool findChessboardCornersRobust(const cv::Mat &origColor, cv::Size boardSize,
                                 std::vector<cv::Point2f> &corners,
                                 const std::string &debugPrefix = "")
{
    corners.clear();
    cv::Mat gray;
    if (origColor.channels() == 3)
        cv::cvtColor(origColor, gray, cv::COLOR_BGR2GRAY);
    else
        gray = origColor.clone();

    // 先用预处理得到更利于检测的灰度图
    cv::Mat processed = preprocessForChessboard(origColor, !debugPrefix.empty(), debugPrefix);

    // 1) 尝试 findChessboardCornersSB（更鲁棒，sector-based）；若 OpenCV 不支持或失败再回退
    bool found = false;
#if CV_VERSION_MAJOR >= 4
    try {
        // some OpenCV builds provide findChessboardCornersSB
        std::vector<cv::Point2f> sbCorners;
        // flags: 0 是合理的默认（也可尝试 cv::CALIB_CB_NORMALIZE_IMAGE | cv::CALIB_CB_ADAPTIVE_THRESH）
        if (cv::findChessboardCornersSB(processed, boardSize, sbCorners))
        {
            // 注意：SB 有时返回子像素级别的坐标，仍然做 cornerSubPix 加精细化
            cv::cornerSubPix(gray, sbCorners, cv::Size(11, 11), cv::Size(-1, -1),
                             cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 40, 0.001));
            corners = sbCorners;
            found = true;
        }
    } catch (...) {
        // 若函数不存在或抛异常则忽略，走回退流程
        found = false;
    }
#endif

    // 2) 如果 SB 不可用或失败，回退到多尺度 findChessboardCorners （从原图与缩放版本尝试）
    if (!found)
    {
        std::vector<double> scales = {1.0, 0.75, 1.5}; // 尝试多尺度
        int cbFlags = cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_NORMALIZE_IMAGE; // 不使用 FAST_CHECK（可能导致错过）
        for (double s : scales)
        {
            cv::Mat imgForDetection;
            double scaleFactor = s;
            if (std::abs(scaleFactor - 1.0) > 1e-3)
            {
                cv::resize(processed, imgForDetection, cv::Size(), scaleFactor, scaleFactor, cv::INTER_LINEAR);
            }
            else
                imgForDetection = processed;

            std::vector<cv::Point2f> tmp;
            bool ok = cv::findChessboardCorners(imgForDetection, boardSize, tmp, cbFlags);
            if (ok)
            {
                // 缩放回原图坐标
                if (std::abs(scaleFactor - 1.0) > 1e-3)
                {
                    for (auto &p : tmp) {
                        p.x /= (float)scaleFactor;
                        p.y /= (float)scaleFactor;
                    }
                }
                // 在原始灰度上进行亚像素精细化
                cv::cornerSubPix(gray, tmp, cv::Size(11, 11), cv::Size(-1, -1),
                                 cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 40, 0.001));
                corners = tmp;
                found = true;
                break;
            }
        }
    }

    // 若找到，在原图上绘制角点便于查看（注意不要修改 origColor 原图）
    return found;
}

// 在图像上绘制文本（支持多行）
void drawText(cv::Mat& img, const std::string& text, cv::Point pos, double fontScale, 
              cv::Scalar color, int thickness, int lineType) {
    cv::putText(img, text, pos, cv::FONT_HERSHEY_SIMPLEX, fontScale, color, thickness, lineType);
}

// 创建结果展示图像
cv::Mat createResultImage(double rms, int validPairs, int totalPairs, cv::Size imageSize, 
                         double baseline, double focal_length, double baseline_pixels) {
    // 创建更大的窗口确保所有文本都能显示
    int resultWindowWidth = 900;
    int resultWindowHeight = 600;
    cv::Mat res(resultWindowHeight, resultWindowWidth, CV_8UC3, cv::Scalar(20, 20, 20));
    
    // 绘制标题 - 使用英文避免乱码
    drawText(res, "Stereo Calibration Completed!", cv::Point(50, 50), 
             1.2, cv::Scalar(0, 255, 0), 3, cv::LINE_AA);
    
    // 绘制详细信息 - 使用更紧凑的布局
    int y_pos = 100;
    int line_height = 30;
    
    std::ostringstream oss;
    oss << std::fixed << std::setprecision(4);
    
    // RMS误差
    oss << "RMS Error: " << rms;
    drawText(res, oss.str(), cv::Point(50, y_pos), 0.7, cv::Scalar(255, 255, 255), 2, cv::LINE_AA);
    y_pos += line_height;
    oss.str("");
    
    // 有效图像对
    oss << "Valid Image Pairs: " << validPairs << "/" << totalPairs;
    drawText(res, oss.str(), cv::Point(50, y_pos), 0.7, cv::Scalar(255, 255, 255), 2, cv::LINE_AA);
    y_pos += line_height;
    oss.str("");
    
    // 图像尺寸
    oss << "Image Size: " << imageSize.width << " x " << imageSize.height;
    drawText(res, oss.str(), cv::Point(50, y_pos), 0.7, cv::Scalar(255, 255, 255), 2, cv::LINE_AA);
    y_pos += line_height;
    oss.str("");
    
    // 基线距离
    oss << "Baseline Distance: " << baseline << " meters";
    drawText(res, oss.str(), cv::Point(50, y_pos), 0.7, cv::Scalar(255, 255, 255), 2, cv::LINE_AA);
    y_pos += line_height;
    oss.str("");
    
    // 添加投影矩阵信息摘要
    oss << "Focal Length: " << focal_length << " pixels";
    drawText(res, oss.str(), cv::Point(50, y_pos), 0.6, cv::Scalar(200, 255, 200), 2, cv::LINE_AA);
    y_pos += line_height;
    oss.str("");
    
    oss << "Baseline: " << baseline_pixels << " pixels";
    drawText(res, oss.str(), cv::Point(50, y_pos), 0.6, cv::Scalar(200, 255, 200), 2, cv::LINE_AA);
    y_pos += line_height;
    oss.str("");
    
    // 绘制文件信息
    drawText(res, "Parameters saved: data/stereo_params.yml", 
             cv::Point(50, y_pos), 0.6, cv::Scalar(200, 200, 255), 2, cv::LINE_AA);
    y_pos += line_height;
    
    drawText(res, "Debug images: debug/", 
             cv::Point(50, y_pos), 0.6, cv::Scalar(200, 200, 255), 2, cv::LINE_AA);
    y_pos += line_height;
    
    // 绘制操作提示
    drawText(res, "Press any key to exit", 
             cv::Point(50, y_pos + 30), 0.8, cv::Scalar(255, 200, 100), 2, cv::LINE_AA);
    
    return res;
}

int main()
{
    std::cout << "==============================================" << std::endl;
    std::cout << "       立体相机标定程序 - 改进版" << std::endl;
    std::cout << "==============================================" << std::endl;
    
    cv::Size boardSize(11, 8); // 内角点数量 (width,height)。确认与你棋盘实际内角点一致
    float squareSize = 0.02f;  // 棋盘方格大小（米）
    
    std::cout << "[配置信息]" << std::endl;
    std::cout << "棋盘格内角点: " << boardSize.width << " x " << boardSize.height << std::endl;
    std::cout << "方格尺寸: " << squareSize << " 米" << std::endl;

    // 检查目录
    std::cout << std::endl << "[目录检查]" << std::endl;
    if (!fs::exists("left_images") || !fs::exists("right_images")) {
        std::cerr << "[错误] left_images 或 right_images 目录不存在" << std::endl;
        std::cerr << "请确保存在以下目录结构:" << std::endl;
        std::cerr << "  left_images/  - 存放左相机图像" << std::endl;
        std::cerr << "  right_images/ - 存放右相机图像" << std::endl;
        return -1;
    }
    
    // 创建调试和数据目录
    if (!fs::exists("debug")) {
        fs::create_directories("debug");
        std::cout << "创建调试目录: debug/" << std::endl;
    }
    if (!fs::exists("data")) {
        fs::create_directories("data");
        std::cout << "创建数据目录: data/" << std::endl;
    }
    std::cout << "目录检查通过" << std::endl;

    // 读取文件并以 ID->filename 的 map 存储，保证左右严格按 ID 匹配
    std::cout << std::endl << "[图像文件扫描]" << std::endl;
    std::map<int, std::string> leftMap, rightMap;
    
    int leftCount = 0, rightCount = 0;
    for (const auto &e : fs::directory_iterator("left_images")) {
        if (!e.is_regular_file()) continue;
        std::string fname = e.path().filename().string();
        std::string ext = e.path().extension().string();
        if (ext != ".jpg" && ext != ".png" && ext != ".jpeg" && ext != ".bmp") continue;
        int id = extractNumberFromFilename(fname);
        if (id < 0) continue;
        leftMap[id] = fname;
        leftCount++;
    }
    for (const auto &e : fs::directory_iterator("right_images")) {
        if (!e.is_regular_file()) continue;
        std::string fname = e.path().filename().string();
        std::string ext = e.path().extension().string();
        if (ext != ".jpg" && ext != ".png" && ext != ".jpeg" && ext != ".bmp") continue;
        int id = extractNumberFromFilename(fname);
        if (id < 0) continue;
        rightMap[id] = fname;
        rightCount++;
    }
    
    std::cout << "左相机图像数量: " << leftCount << std::endl;
    std::cout << "右相机图像数量: " << rightCount << std::endl;

    // 取左右 ID 的交集并按 ID 升序处理
    std::vector<int> ids;
    for (const auto &p : leftMap) {
        if (rightMap.find(p.first) != rightMap.end()) ids.push_back(p.first);
    }
    std::sort(ids.begin(), ids.end());
    
    std::cout << "可配对的图像对数量: " << ids.size() << std::endl;
    
    if (ids.size() < 5) {
        std::cerr << std::endl << "[错误] 有效配对少于 5 对，无法进行稳定标定" << std::endl;
        std::cerr << "请检查以下事项:" << std::endl;
        std::cerr << "1. 确保左右目录中的图像文件名末尾有相同的数字ID（例如: left_01.jpg 和 right_01.jpg）" << std::endl;
        std::cerr << "2. 确保图像格式支持（jpg, png, jpeg, bmp）" << std::endl;
        std::cerr << "3. 确保左右相机图像数量足够且能正确配对" << std::endl;
        return -1;
    }

    std::vector<std::vector<cv::Point2f>> imgPointsL, imgPointsR;
    std::vector<std::vector<cv::Point3f>> objPoints;
    cv::Size imageSize;
    bool firstImage = true;
    int validPairs = 0;

    std::cout << std::endl << "[开始棋盘格检测]" << std::endl;
    std::cout << "正在自动处理所有图像，请稍候..." << std::endl;

    // 创建显示窗口
    cv::namedWindow("Left Camera - Chessboard Detection", cv::WINDOW_NORMAL);
    cv::namedWindow("Right Camera - Chessboard Detection", cv::WINDOW_NORMAL);

    for (int id : ids) {
        std::string leftFile = "left_images/" + leftMap[id];
        std::string rightFile = "right_images/" + rightMap[id];
        std::cout << "处理 ID=" << id << " : " << leftMap[id] << " | " << rightMap[id] << " ... ";

        cv::Mat imgL = cv::imread(leftFile, cv::IMREAD_COLOR);
        cv::Mat imgR = cv::imread(rightFile, cv::IMREAD_COLOR);
        if (imgL.empty() || imgR.empty()) {
            std::cout << "读取失败，跳过" << std::endl;
            continue;
        }

        // 记录原始尺寸（用于 stereoCalibrate）
        if (firstImage) {
            imageSize = imgL.size();
            std::cout << std::endl << "[图像尺寸] " << imageSize.width << " x " << imageSize.height << std::endl;
            firstImage = false;
            
            // 设置窗口大小适应图像尺寸
            cv::resizeWindow("Left Camera - Chessboard Detection", imageSize.width / 2, imageSize.height / 2);
            cv::resizeWindow("Right Camera - Chessboard Detection", imageSize.width / 2, imageSize.height / 2);
        }

        // 创建用于显示的副本（不修改原图）
        cv::Mat displayL = imgL.clone();
        cv::Mat displayR = imgR.clone();

        // 对左右图进行检测（在原始分辨率上）
        std::vector<cv::Point2f> cornersL, cornersR;
        std::string debugPrefixL = "debug/id_" + std::to_string(id) + "_L";
        std::string debugPrefixR = "debug/id_" + std::to_string(id) + "_R";

        bool okL = findChessboardCornersRobust(imgL, boardSize, cornersL, debugPrefixL);
        bool okR = findChessboardCornersRobust(imgR, boardSize, cornersR, debugPrefixR);

        if (okL && okR) {
            std::cout << "成功检测到棋盘格" << std::endl;
            
            // 在原图上绘制角点（使用原图坐标）
            cv::drawChessboardCorners(displayL, boardSize, cornersL, true);
            cv::drawChessboardCorners(displayR, boardSize, cornersR, true);

            // 保存点集合
            imgPointsL.push_back(cornersL);
            imgPointsR.push_back(cornersR);

            // 对象点（世界坐标 Z=0）
            std::vector<cv::Point3f> obj;
            for (int r = 0; r < boardSize.height; ++r)
                for (int c = 0; c < boardSize.width; ++c)
                    obj.push_back(cv::Point3f(c * squareSize, r * squareSize, 0));
            objPoints.push_back(obj);

            validPairs++;

            // 在图像上添加文本信息 - 使用英文避免乱码
            drawText(displayL, "Detection Success - Auto Continue", cv::Point(10, 30), 
                     1.0, cv::Scalar(0, 255, 0), 2, cv::LINE_AA);
            drawText(displayR, "Detection Success - Auto Continue", cv::Point(10, 30), 
                     1.0, cv::Scalar(0, 255, 0), 2, cv::LINE_AA);

            // 显示窗口（短暂显示，不等待用户输入）
            cv::imshow("Left Camera - Chessboard Detection", displayL);
            cv::imshow("Right Camera - Chessboard Detection", displayR);
            
            // 短暂显示图像，允许用户按ESC中断
            int key = cv::waitKey(300); // 显示300ms后自动继续
            if (key == 27) {
                std::cout << "[用户中断] 检测过程被用户中断" << std::endl;
                break;
            }
        } else {
            std::cout << "检测失败" << std::endl;
            // 在图像上添加失败信息 - 使用英文避免乱码
            drawText(displayL, "Detection Failed", cv::Point(10, 30), 
                     1.0, cv::Scalar(0, 0, 255), 2, cv::LINE_AA);
            drawText(displayR, "Detection Failed", cv::Point(10, 30), 
                     1.0, cv::Scalar(0, 0, 255), 2, cv::LINE_AA);
            
            // 显示失败图像（短暂显示）
            cv::imshow("Left Camera - Chessboard Detection", displayL);
            cv::imshow("Right Camera - Chessboard Detection", displayR);
            cv::waitKey(200); // 短暂显示200ms后自动继续
            
            // 保存原始与处理后的调试图像（易于离线分析）
            cv::imwrite("debug/fail_left_id_" + std::to_string(id) + ".png", imgL);
            cv::imwrite("debug/fail_right_id_" + std::to_string(id) + ".png", imgR);
        }
    }

    cv::destroyAllWindows();

    std::cout << std::endl << "[检测结果统计]" << std::endl;
    std::cout << "总配对图像对: " << ids.size() << std::endl;
    std::cout << "成功检测对: " << validPairs << std::endl;
    
    if (validPairs < 5) {
        std::cerr << std::endl << "[错误] 有效配对少于 5 对，无法进行稳定立体标定" << std::endl;
        std::cerr << "可能原因:" << std::endl;
        std::cerr << "1. 棋盘格图案不清晰或光照条件差" << std::endl;
        std::cerr << "2. 棋盘格被部分遮挡" << std::endl;
        std::cerr << "3. 相机对焦不准确" << std::endl;
        std::cerr << "请检查 debug 目录中保存的失败图像以分析原因" << std::endl;
        return -1;
    }

    // 开始立体标定
    std::cout << std::endl << "[开始立体标定计算]" << std::endl;
    cv::Mat M1 = cv::Mat::eye(3, 3, CV_64F), D1 = cv::Mat::zeros(5, 1, CV_64F);
    cv::Mat M2 = cv::Mat::eye(3, 3, CV_64F), D2 = cv::Mat::zeros(5, 1, CV_64F);
    cv::Mat R, T, E, F;
    int flags = cv::CALIB_FIX_ASPECT_RATIO + cv::CALIB_ZERO_TANGENT_DIST + cv::CALIB_USE_INTRINSIC_GUESS;
    cv::TermCriteria criteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 200, 1e-6);

    std::cout << "正在进行立体标定计算，请稍候..." << std::endl;
    double rms = cv::stereoCalibrate(objPoints, imgPointsL, imgPointsR,
                                     M1, D1, M2, D2, imageSize, R, T, E, F,
                                     flags, criteria);
    std::cout << "立体标定完成，重投影误差 RMS = " << rms << std::endl;

    // 立体校正
    std::cout << "计算立体校正参数..." << std::endl;
    cv::Mat R1, R2, P1, P2, Q;
    cv::stereoRectify(M1, D1, M2, D2, imageSize, R, T, R1, R2, P1, P2, Q);

    // 保存参数
    std::cout << "保存标定参数到文件..." << std::endl;
    cv::FileStorage fsout("data/stereo_params.yml", cv::FileStorage::WRITE);
    if (!fsout.isOpened()) {
        std::cerr << "[错误] 无法写入 data/stereo_params.yml" << std::endl;
        return -1;
    }
    fsout << "M1" << M1 << "D1" << D1 << "M2" << M2 << "D2" << D2;
    fsout << "R" << R << "T" << T << "R1" << R1 << "R2" << R2;
    fsout << "P1" << P1 << "P2" << P2 << "Q" << Q;
    fsout.release();

    // 输出详细的标定结果信息
    std::cout << std::endl << "==============================================" << std::endl;
    std::cout << "           立体标定结果报告" << std::endl;
    std::cout << "==============================================" << std::endl;
    std::cout << "重投影误差 (RMS): " << rms << std::endl;
    std::cout << "有效标定图像对: " << validPairs << "/" << ids.size() << std::endl;
    std::cout << "图像尺寸: " << imageSize.width << " x " << imageSize.height << std::endl;
    std::cout << std::endl;
    
    std::cout << "左相机内参矩阵 M1:" << std::endl;
    std::cout << M1 << std::endl;
    
    std::cout << "右相机内参矩阵 M2:" << std::endl;
    std::cout << M2 << std::endl;
    
    std::cout << "左相机畸变系数 D1:" << std::endl;
    std::cout << D1 << std::endl;
    
    std::cout << "右相机畸变系数 D2:" << std::endl;
    std::cout << D2 << std::endl;
    
    std::cout << "旋转矩阵 R:" << std::endl;
    std::cout << R << std::endl;
    
    std::cout << "平移向量 T:" << std::endl;
    std::cout << T << std::endl;
    
    std::cout << "本征矩阵 E:" << std::endl;
    std::cout << E << std::endl;
    
    std::cout << "基础矩阵 F:" << std::endl;
    std::cout << F << std::endl;
    
    std::cout << "左相机校正旋转矩阵 R1:" << std::endl;
    std::cout << R1 << std::endl;
    
    std::cout << "右相机校正旋转矩阵 R2:" << std::endl;
    std::cout << R2 << std::endl;
    
    std::cout << "左相机投影矩阵 P1:" << std::endl;
    std::cout << P1 << std::endl;
    
    std::cout << "右相机投影矩阵 P2:" << std::endl;
    std::cout << P2 << std::endl;
    
    std::cout << "重投影矩阵 Q:" << std::endl;
    std::cout << Q << std::endl;
    
    // 计算基线距离（两个相机之间的距离）
    double baseline = cv::norm(T);
    std::cout << "相机基线距离: " << baseline << " 米" << std::endl;
    
    // 输出投影矩阵的详细信息
    std::cout << std::endl << "[投影矩阵详细信息]" << std::endl;
    std::cout << "左相机投影矩阵 P1 (3x4):" << std::endl;
    std::cout << "  fx: " << P1.at<double>(0,0) << ", fy: " << P1.at<double>(1,1) << std::endl;
    std::cout << "  cx: " << P1.at<double>(0,2) << ", cy: " << P1.at<double>(1,2) << std::endl;
    std::cout << "  Tx: " << P1.at<double>(0,3) << std::endl;
    
    std::cout << "右相机投影矩阵 P2 (3x4):" << std::endl;
    std::cout << "  fx: " << P2.at<double>(0,0) << ", fy: " << P2.at<double>(1,1) << std::endl;
    std::cout << "  cx: " << P2.at<double>(0,2) << ", cy: " << P2.at<double>(1,2) << std::endl;
    std::cout << "  Tx: " << P2.at<double>(0,3) << std::endl;
    
    // 计算视差到深度的转换参数
    double focal_length = P2.at<double>(0,0); // 使用右相机的焦距
    double baseline_pixels = -P2.at<double>(0,3) / focal_length; // 基线（像素单位）
    std::cout << "视差转换参数:" << std::endl;
    std::cout << "  焦距: " << focal_length << " 像素" << std::endl;
    std::cout << "  基线: " << baseline_pixels << " 像素" << std::endl;
    std::cout << "  Q矩阵参数: " << Q.at<double>(3,2) << " (用于深度计算: Z = focal_length * baseline / disparity)" << std::endl;
    
    std::cout << std::endl << "参数文件已保存: data/stereo_params.yml" << std::endl;
    std::cout << "调试图像已保存: debug/" << std::endl;

    // 创建结果展示图像（使用英文避免乱码）
    cv::Mat resultImage = createResultImage(rms, validPairs, ids.size(), imageSize, 
                                           baseline, focal_length, baseline_pixels);
    
    // 创建窗口并显示结果
    cv::namedWindow("Calibration Result", cv::WINDOW_NORMAL);
    cv::resizeWindow("Calibration Result", 900, 600);
    cv::imshow("Calibration Result", resultImage);
    
    std::cout << std::endl << "按任意键退出程序..." << std::endl;
    cv::waitKey(0);  // 只在最后等待用户按键
    cv::destroyAllWindows();

    std::cout << std::endl << "立体标定程序执行完成!" << std::endl;
    return 0;
}